/**
 * @file findline.c
 * @author sevenfite(sevenfite@163.com)
 * @brief 巡线方法
 * @version 0.1
 * @date 2024-01-09
 *
 * @copyright Copyright (c) 2024
 *
 *
 */

#include "findline.h"
#include "control.h"
#include "motor.h"
#include "pid.h"
#include "C_code_Log.h"
#include "camera.h"
static PIDTypeDef gl_sensorPID;  // 传感器PID


/// @brief 巡线测试
/// @param rank 以80为中心的线的位置 0-160（由摄像头传入过来的数）
void findLine_test(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    PID_Init(&gl_sensorPID, 80, 1000, 20, 0,
             100);  // 对速度PID的参数进行初始化设置
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1000, 1000, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 基础巡线方法
 * 
 * @param rank 左右偏差等级，0-160，80为中心
 * @param speed 期望平均速度，单位：PWM值
 * @param limit 速度限制，单位：PWM值
 * @param kp pid比例系数
 * @param kd pid微分系数
 * @param difSpeedOfFrontBack 前后轮速度差，单位：PWM值，在两轮差速模式下此值无效
 * @param LRdif 左右轮速度差，单位：PWM值，右减左的固有速度差
 */
static void flMethod_Base(int rank,int speed,int limit,int kp,int kd,int difSpeedOfFrontBack,int LRdif) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    if ((cameraCmd&0xFC) != CAMERA_CMD_FIND_LINE) {//&&0xFC是为了去掉找点的标志位,即去掉后两位
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, limit, kp, 0, kd);
    gl_controlCenter.findLineFlag = 1;
  }
  // if(rank<60){
  //   rank=rank*7/6;
  // }else if(rank<100){
  //   rank=70+(rank-60)/2;
  // }else{
  //   rank=90+(rank-100)*7/6;
  // }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  LRdif=LRdif/2;
#ifdef TWO_WHEEL
  positionSpeedOut(speed-LRdif, speed+LRdif, difSpeed);  // 位置式改变电机速度
#else
  positionSpeedOut_2(speed-LRdif,speed+LRdif,difSpeed,difSpeedOfFrontBack);
#endif
}
/**

* 函数介绍：缺省巡线方法
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的PID三个参数
* 作者    ：@断忆

*/
static void flMethod_default(int rank) {
  flMethod_Base(rank,1300,2000,40,12,0,0);
}
/**

* 函数介绍：慢速巡线
* 输入参数：偏差值
* 输出参数：无
* 返回值  ：
* 其他    ：
* 作者    ：

*/
static void flMethod_slow(int rank) {
  flMethod_Base(rank,500,400,40,8,0,0);
}
/**

* 函数介绍：中速巡线方法
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的PID三个参数
* 作者    ：@断忆

*/
static void flMethod_middle(int rank) {
  int speed = 0;
  speed = 1800 - abs16(rank - 80) * 10;
  flMethod_Base(rank,speed,2000,50,20,0,0);
}
/**
 * @brief 快速巡线
 * 
 * @param rank 
 */
static void flMethod_quick(int rank) {
  flMethod_Base(rank,1500,2000,60,15,0,0);
}
/**

* 函数介绍：用在下平台巡线（FL_DownPlatform）
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的PID三个参数
* 作者    ：@断忆

*/
static void flMethod_downplatform(int rank) {
  flMethod_Base(rank,2300,1000,75,24,300,0);
}
/**
 * @brief 准备下平台,还在平台上巡线
 * 
 * @param rank 
 */
static void flMethod_preDown(int rank) {
  //改这里的速度会影响下珠峰与下山丘的一个时间参数
  flMethod_Base(rank,1000,1000,75,24,100,0);
}
/**

* 函数介绍：下长台巡线（FL_DOWN_LONG）
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的PID三个参数
* 作者    ：@断忆

*/
static void flMethod_down_long(int rank) {
  flMethod_Base(rank,1600,1000,60,15,0,-200);
}
/**

* 函数介绍：下长台巡线SLOW（FL_DOWN_LONG）
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的PID三个参数
* 作者    ：@断忆

*/
static void flMethod_down_long_slow(int rank) {
  flMethod_Base(rank,1000,1000,50,10,0,-200);
}
/**
 * @brief 上长台巡线（FL_UP_LONG）
 * 
 * @param rank 
 */
static void flMethod_up_long(int rank) {
  flMethod_Base(rank,2150,1200,40,12,0,-100);
}
/**
 * @brief 上长台巡线slow（FL_UP_LONG）
 * 
 * @param rank 
 */
static void flMethod_up_long_slow(int rank) {
  flMethod_Base(rank,1500,600,40,12,0,0);
}

/**

* 函数介绍：倒车巡线
* 输入参数：偏差值
* 输出参数：无
* 返回值  ：
* 其他    ：
* 作者    ：

*/
static void flMethod_Reversing(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    if (cameraCmd != CAMERA_CMD_FIND_LINE_BACK) {
      camera_SendCommand(CAMERA_CMD_FIND_LINE_BACK);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1000, 40, 0, 25);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = +positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(-1000, -1000, difSpeed);  // 位置式改变电机速度
}
/**

* 函数介绍：台阶巡线
* 输入参数：偏差值
* 输出参数：无
* 返回值  ：
* 其他    ：
* 作者    ：

*/
static void flMethod_Step(int rank) {
  flMethod_Base(rank,1500,800,70,18,500,0);
}
/**

* 函数介绍：过桥巡线方法
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的P I
D三个参数
* 作者    ：@断忆

*/
static void flMethod_bridge(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if (cameraCmd != CAMERA_CMD_BRIDGE) {
      camera_SendCommand(CAMERA_CMD_BRIDGE);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1000, 50, 0, 5);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(2000, 2000, difSpeed);  // 位置式改变电机速度
}
static void flMethod_bridge_slow(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if (cameraCmd != CAMERA_CMD_BRIDGE) {
      camera_SendCommand(CAMERA_CMD_BRIDGE);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1000, 30, 0, 5);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
#ifdef TWO_WHEEL
  positionSpeedOut(600, 600, difSpeed);  // 位置式改变电机速度
#else
  positionSpeedOut_2(1000, 1000, difSpeed, 150);  // 位置式改变电机速度
#endif
}

/**
 * @brief 圆弧的寻线方法
 * 
 * @param rank 
 */
static void flMethod_arc(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if ((cameraCmd&0xFC) != CAMERA_CMD_FIND_LINE) {
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1000, 80, 0, 15);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1600, 1800, difSpeed);  // 位置式改变电机速度
}

/**
 * @brief 在跷跷板的巡线
 * 
 * @param rank 
 */
static void flMethod_seesaw(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if (cameraCmd != CAMERA_CMD_SEESAW) {
      camera_SendCommand(CAMERA_CMD_SEESAW);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1000, 40, 0, 5);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1500, 1500, difSpeed);  // 位置式改变电机速度
}
static void flMethod_seesaw_slow(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if (cameraCmd != CAMERA_CMD_SEESAW) {
      camera_SendCommand(CAMERA_CMD_SEESAW);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 400, 25, 0, 5);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(700, 900, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 抓线
 * 
 * @param rank 
 */
static void flMethod_trace(int rank,Direction dir) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if (dir == Left) {
      if (cameraCmd != TRACE_L_CMD_FIND_LINE) {
        camera_SendCommand(TRACE_L_CMD_FIND_LINE);
        return;  // 摄像头数据错误，直接退出
      }
    }
    else {
      if (cameraCmd != TRACE_R_CMD_FIND_LINE) {
        camera_SendCommand(TRACE_R_CMD_FIND_LINE);
        return;  // 摄像头数据错误，直接退出
      }
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 75, 0, 35);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1000, 1000, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 波浪板的寻线
 * 
 * @param rank 
 */
static void flMethod_Wave(int rank) {
  flMethod_Base(rank,1500,1200,60,10,0,0);
}
/**
 * @brief 刀山寻线
 * 
 * @param rank 偏差输入
 * 
 */
static void flMethod_knife(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    if (cameraCmd != KNIFE_CMD_FIND_LINE) {
      camera_SendCommand(KNIFE_CMD_FIND_LINE);
      cameraCmd = KNIFE_CMD_FIND_LINE;
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 70, 0, 18);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1200, 1200, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 高台校正寻线
 * 
 * @param rank 偏差输入
 * 
 */
static void flMethod_High_platform_adjust(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1700, 110, 0, 23);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(700, 700, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 低台校正寻线
 * 
 * @param rank 偏差输入
 * 
 */
static void flMethod_Low_platform_adjust(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1400, 100, 0, 23);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1000, 1000, difSpeed);  // 位置式改变电机速度
}
/**

* 函数介绍：慢速上平台
* 输入参数：偏差值
* 输出参数：无
* 返回值  ：
* 其他    ：
* 作者    ：

*/
static void flMethod_up_platform(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    if ((cameraCmd&0xFC) != CAMERA_CMD_FIND_LINE) {//&&0xFC是为了去掉找点的标志位,即去掉后两位
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 800, 60, 0, 8);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(1000, 1000, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 加速时候的巡线
 * 
 * @param rank 
 */
void flMethod_speedUp(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if ((cameraCmd&0xFC) != CAMERA_CMD_FIND_LINE) {//&&0xFC是为了去掉找点的标志位,即去掉后两位
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      return;//摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 20, 0, 10);
    gl_controlCenter.findLineFlag = 1;
  }
  //得出此时的目标速度值
  extern int16_t getSpeedOfAccelerate();
  int16_t speed = getSpeedOfAccelerate();
  // if(abs16(rank-80)<10){
  //   gl_sensorPID.kp=15;
  //   gl_sensorPID.kd=10;
  // }
  // else if(abs16(rank-80)<20){
  //   gl_sensorPID.kp=14;
  //   gl_sensorPID.kd=9;
  // }
  // else {
  //   gl_sensorPID.kp=13;
  //   gl_sensorPID.kd=9;
  // }
  if(abs16(rank-80)<10){
    gl_sensorPID.kp=14;
    gl_sensorPID.kd=7;
  }
  else if(abs16(rank-80)<20){
    gl_sensorPID.kp=14;
    gl_sensorPID.kd=7;
  }
  else {
    gl_sensorPID.kp=15;
    gl_sensorPID.kd=7;
  }
  int pidSpeed = limit(speed,3000,6000);
  gl_sensorPID.kp = gl_sensorPID.kp  * 5000 /pidSpeed;
  gl_sensorPID.kd = gl_sensorPID.kd  * 5000 /pidSpeed;
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出

  positionSpeedOut(speed, speed, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 加速后退巡线
 * 
 * @param rank 
 */
void flMethod_speedUpReversing(int rank) {
  int difSpeed;
  // 保证每个路段初始化一次
  if (gl_controlCenter.findLineFlag == 0) {
    if (cameraCmd != CAMERA_CMD_FIND_LINE_BACK) {
      camera_SendCommand(CAMERA_CMD_FIND_LINE_BACK);
      return;//摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 700, 45, 0, 25);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = +positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  extern int16_t getSpeedOfAccelerate();
  int16_t speed = getSpeedOfAccelerate();
  positionSpeedOut(speed, speed, difSpeed);  // 位置式改变电机速度
}
/**

* 函数介绍：高速
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：每个方法最主要的区别：1、glmotorSpeed初始速度 2、glsensorPID的PID三个参数
* 作者    ：@断忆

*/
static void flMethod_speedBest(int rank) {
  int difSpeed;
  if (gl_controlCenter.findLineFlag == 0)  // 保证每个路段初始化一次
  {
    if ((cameraCmd&0xFC) != CAMERA_CMD_FIND_LINE) {//&&0xFC是为了去掉找点的标志位,即去掉后两位
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      return;  // 摄像头数据错误，直接退出
    }
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 11, 0, 5);
    gl_controlCenter.findLineFlag = 1;
  }
  difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(5000, 5000, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 根据编码器的值来调整速度
 * 
 * @param speed 电机的目标速度，正数为前进，负数为后退
 * @note 电机的第三种调整速度的方式
 * 
 */
void flMethod_encoder(int16_t speed){
  if (gl_controlCenter.findLineFlag == 0) {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 700, 50, 0, 25);
    gl_controlCenter.findLineFlag = 1;
  }
  int rank =80+ (gl_motor.encoderRightFront - gl_motor.encoderLeftFront)*150
  /abs16(gl_motor.encoderRightFront + gl_motor.encoderLeftFront);
  int difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(speed, speed, difSpeed);  // 位置式改变电机速度
}
#include "gyro.h"
/**
 * @brief 根据陀螺仪的值来走直线
 * 
 * @param angle 目标角度
 * @param speed 电机的目标速度，正数为前进，负数为后退
 */
void alongGyro(float angle,int16_t speed){
  if (gl_controlCenter.findLineFlag == 0) {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 60, 0, 25);
    gl_controlCenter.findLineFlag = 1;
    //初始化角度,获取当前角度
  }
  int rank =80+ errCalculate(gyro_getYaw(),angle,Right);
  int difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(speed, speed, difSpeed);  // 位置式改变电机速度
}
/**
 * @brief 根据陀螺仪的值来走直线
 * 
 * @param speed 电机的目标速度，正数为前进，负数为后退
 * @note 刚开始调用的时候会记录朝向，然后沿着朝向走直线
 * 
 */
void flMethod_gyro(int16_t speed){
  static float angle = 0;
  if (gl_controlCenter.findLineFlag == 0) {
    //初始化角度,获取当前角度
    angle = gyro_getYaw();
  }
  alongGyro(angle,speed);
}
/**
 * @brief 根据陀螺仪的值来走直线
 * 
 * @param speed 电机的目标速度，正数为前进，负数为后退
 * @note 刚开始调用的时候会记录朝向，然后沿着朝向走直线
 * 
 */
void flMethod_gyro_UP(int16_t speed){
  static float angle = 0;
  if (gl_controlCenter.findLineFlag == 0) {
    //初始化角度,获取当前角度
    angle = gyro_getYaw();
  }
    if (gl_controlCenter.findLineFlag == 0) {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 60, 0, 25);
    gl_controlCenter.findLineFlag = 1;
    //初始化角度,获取当前角度
  }
  int rank =80+ errCalculate(gyro_getYaw(),angle,Right);
  int difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
  positionSpeedOut(speed+300, speed, difSpeed);  // 位置式改变电机速度
}

/*------------------------控制台----------------*/
/// @brief 巡线任务
/// @param control 控制台
/// @note 通过控制台的findLineWay来选择使用哪个巡线方法
/// @note 这个巡线方法是有可能在加速过程或者过障碍中被修改的
void findLine_Task(const ControlCenterTypeDef *control) {
  // 如果巡线任务状态为准备就绪
  if (control->runState->F_LineState == MY_READY) {
    // 从摄像头获取线的位置的等级
    int findLineRank = getLineRank();
    // 根据巡线任务的类型执行不同的巡线任务
    switch (control->findLineWay) {
      case FL_default:
        flMethod_default(findLineRank);
        break;
      case FL_slow:
        flMethod_slow(findLineRank);
        break;
      case FL_middle:
        flMethod_middle(findLineRank);
        break;
      case FL_quick:
        flMethod_quick(findLineRank);
        break;
      case FL_Reversing:
        flMethod_Reversing(findLineRank);
        break;
      case FL_Step:
        flMethod_Step(findLineRank);
        break;
      case FL_WAVE:
        flMethod_Wave(findLineRank);
        break;
      case FL_KNIFE:
        flMethod_knife(findLineRank);
        break;
      case FL_low_adjust:
        flMethod_Low_platform_adjust(findLineRank);
        break;
      case FL_high_adjust:
        flMethod_High_platform_adjust(findLineRank);
        break;
      case FL_DownPlatform:
        flMethod_downplatform(findLineRank);
        break;
      case FL_PreDown:
        flMethod_preDown(findLineRank);
        break;
      case FL_UP_LONG:
        flMethod_up_long(findLineRank);
        break;
      case FL_UP_LONG_SLOW:
        flMethod_up_long_slow(findLineRank);
        break;
      case FL_DOWN_LONG:
        flMethod_down_long(findLineRank);
      //未实现
        break;
      case FL_DOWN_LONG_SLOW:
        flMethod_down_long_slow(findLineRank);
        break;
      case FL_bridge:
        flMethod_bridge(findLineRank);
        break;
      case FL_bridge_slow:
        flMethod_bridge_slow(findLineRank);
        break;
      case FL_Arc:
        flMethod_arc(findLineRank);
        break;
      case FL_seesaw:
        flMethod_seesaw(findLineRank);
        break;
      case FL_seesaw_slow:
        flMethod_seesaw_slow(findLineRank);
        break;
      case FL_TRACE_L:
        flMethod_trace(findLineRank,Left);
        break;
      case FL_speedUp:
        flMethod_speedUp(findLineRank);
        break;
      case FL_speedUp_Back:
        flMethod_speedUpReversing(findLineRank);
        break;
      case FL_speedBest:
        flMethod_speedBest(findLineRank);
        break;
      case FL_No:
        break;
      case NFL_Platform:
        speedAdjustment(700, 700);
        break;
      case NFL_Bridge:
        speedAdjustment(1000, 1200);
        break;
      case NFL_Slow:
        speedAdjustment(1000, 1000);
        break;
      case NFL_Arc:
        speedAdjustment(700, 1600);
        break;
      case NFL_Back:
        speedAdjustment(-1500, -1500);
        break;
      case NFL_GYRO_UP:
        flMethod_gyro_UP(1500);
        break;
      case NFL_GYRO_UP_PLATFORM:
        flMethod_gyro(1400);
        break;
      case NFL_GYRO:
        flMethod_gyro(1200);
        break;
      case NFL_GYRO_DOWN:
        flMethod_gyro(1000);
        break;
      case NFL_GYRO_BACK:
        flMethod_gyro(-1200);
        break;
      default:
        break;
    }
  }
}



/*-------------Test--------------*/
#include "display.h"
#include "usart_port.h"
#include "ABS_key.h"
#include <stdlib.h>
#include "Mydelay.h"
/// @brief 巡线测试
/// @param key_Num
/// @note 从摄像头获取数据，解码后得到线的位置，再用findLine_test()函数进行测试
/// @return
My_StatusTypeDef findLine(int key_Num) {
  static uint8_t flag;
  static uint8_t findLine; 
  static int rank;
  #define MAX_ITEM 5
  if(flag ==0)
  {
    flag = 1;
    findLine = 0;
    rank = 80;
    displayStr("default", Display_firstCol + Display_OneChar, Display_secondRow,RED);
    displayStr("bridge", Display_firstCol + Display_OneChar, Display_thirdRow,RED);
    displayStr("traceR", Display_firstCol + Display_OneChar, Display_fourthRow,RED);
    displayStr("middle",Display_firstCol + Display_OneChar, Display_fifthRow,RED);
    displayStr("base_test",Display_firstCol + Display_OneChar, Display_fifthRow+Display_spaced,RED);
  }
  displayStr(">", Display_firstCol, Display_secondRow + findLine * Display_spaced,RED);
  char str[20];
  sprintf(str, "lineRank:%d  ", getLineRank());
  displayStr(str, Display_firstCol, Display_firstRow, RED);
  
  if (USART_DataIsReceived(&usartCamera) == true) {
    USART_DataResetReceivedFlag(&usartCamera);
    rank = getLineRank();
  }

  if (key_Num == Key_ESC) {
    flag = 0;
    return MY_OK;
  }
  else if(key_Num == Key_UP&&flag==1){
     displayStr(" ", Display_firstCol, Display_secondRow + findLine * Display_spaced,RED);
    findLine +=((MAX_ITEM)-1);
    findLine %=MAX_ITEM ;
  }
  else if(key_Num == Key_Down&&flag==1){
     displayStr(" ", Display_firstCol, Display_secondRow + findLine * Display_spaced,RED);
    findLine ++;
    findLine %=MAX_ITEM;
  }
  else if(key_Num == Key_OK&&flag==1){
    gl_controlCenter.findLineFlag=0;
    flag =2;
  }
  if(flag==2){
    if(myDelay((uint32_t)findLine,3000)==true){
      flag = 1;
      motorStop();
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
    }
    else{
      switch (findLine)
      {
      case 0 :
        flMethod_default(rank);
        break;
      case 1:
        flMethod_bridge(rank);
      case 2:
        if(gl_controlCenter.findLineFlag==0){
          gl_controlCenter.findLineFlag=1;
          PID_Init(&gl_sensorPID, 80, 2000, 100, 0, 18);
        }
        if(cameraCmd!=TRACE_R_CMD_FIND_LINE)
          {camera_SendCommand(TRACE_R_CMD_FIND_LINE);}
        else
        {int difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
        positionSpeedOut(1500, 1500, difSpeed);  // 位置式改变电机速度
        }
        break;
      case 3:
        flMethod_middle(rank);
        break;
      case 4:
      {
      extern int gl_value1;
      extern int gl_value2;
      extern int gl_value3;
      extern int gl_value4;
      extern int gl_value5;
      extern int gl_value6;
      int speed = 0;
      speed = 1800 - abs16(rank - 80) * 10;
        flMethod_Base(rank,gl_value1,gl_value2,gl_value3,gl_value4,0,0);
        break;
      }
      default:
        break;
      }
      
    }
  }
  return MY_BUSY;
#undef MAX_ITEM
}


